#include <lversion.h>
#if L_VERSION_MODULE_BV_TREE == 120104

#include "_lbvTree_120104.h"

#include <lrandom.h>
using namespace llib;
using namespace llib::geom;

#include <queue>
using namespace std;

#include <boost/assert.hpp>

BVTreeIter* BVTreeIter::Create( BVTree& tree, TraversalType type )
{
    BVTreeIter* iter;
    switch (type) {
        case Preorder:  iter = new BVTreeIter_Preorder();   break;
        case Inorder:   iter = new BVTreeIter_Inorder();    break;
        case Postorder: iter = new BVTreeIter_Postorder();  break;
        case Hierarchy: BOOST_ASSERT(FALSE);                break; // not implement
        case LeafOnly:  BOOST_ASSERT(FALSE);                break; // not implement
        default:        BOOST_ASSERT(FALSE);                break; // not implement
    }
    iter->_tree = &tree;
    iter->reset();  //Every thing is ready when iterator is newly created
    return iter;
}

BVNode* BVTreeIter_Preorder::Next()
{
    if(_stack.empty()) return NULL;

    BVNode* res = _stack.back();
    _stack.pop_back();

    BVNode* left = res->GetChildLeft();
    if(left != NULL) _stack.push_back(left);
    BVNode* right = res->GetChildRight();
    if(right != NULL) _stack.push_back(right);

    return res;
}

void BVTreeIter_Preorder::reset()
{
    _stack.clear();
    BVNode* root = _tree->GetRoot();
    _stack.push_back(root);
}

BVNode* BVTreeIter_Inorder::Next()
{
    if(_stack.empty()) return NULL;

    BVNode* res = _stack.back();
    _stack.pop_back();

    BVNode* node = res->GetChildRight();
    while(node != NULL) {
        _stack.push_back(node);
        node = node->GetChildLeft();
    }

    return res;
}

void BVTreeIter_Inorder::reset()
{
    _stack.clear();
    BVNode* node = _tree->GetRoot();
    while(node != NULL) {
        _stack.push_back(node);
        node = node->GetChildLeft();
    }
}

BVNode* BVTreeIter_Postorder::Next()
{
    if(_stack.empty()) return NULL;

    BVNode* res = NULL;
    BVNode *node, *right;
    while(res == NULL) {
        node = _stack.back();
        right = node->GetChildRight();
        if(right == NULL || right == _lastPop) {
            _stack.pop_back();
            res = node;
            _lastPop = node;
        } else {
            node = right;
            while(node != NULL) {
                _stack.push_back(node);
                node = node->GetChildLeft();
            }
        }
    }

    return res;
}

void BVTreeIter_Postorder::reset()
{
    _stack.clear();
    _lastPop = NULL;
    BVNode* node = _tree->GetRoot();
    while(node != NULL) {
        _stack.push_back(node);
        node = node->GetChildLeft();
    }
}

BVTree::~BVTree()
{
    delete[] _nodeList;
    delete[] _leafIndexList;
}

void BVTree::Init( TriMesh& triMesh )
{
    _mesh = &triMesh;
    /* collect data for tree building up */
    vector<Vec3f> triCenter;
    Int32 faceNum = triMesh.GetFaceNum();
    Vec3f center;
    MeshFace face;
    FOR(F,0,faceNum) {
        face = triMesh.GetFace(F);
        center = ( face.GetVertex(0).GetRef() +
            face.GetVertex(1).GetRef() + 
            face.GetVertex(2).GetRef() ) / 3;
        triCenter.push_back(center);
    }
    /* build */
    build(triCenter);
}

// We build a complete binary tree as BVH
// thus tree can be arranged in array
void BVTree::build( const std::vector<Vec3f>& elementPostions )
{
    BOOST_ASSERT(SZ(elementPostions) > 0);

    // remove old data
    delete[] _nodeList;
    delete[] _leafIndexList;

    /* index pointer to elementPostions */
    vector<Int32> leafIndex(SZ(elementPostions));
    FOR(I,0,SZ(leafIndex)) leafIndex[I] = I;
    /* arrange elements by index pointer */
    leafArrangement(elementPostions, leafIndex);

    /* the node numbers */
    _leafNum = SZ(elementPostions);
    _nodeNum = _leafNum * 2 - 1;
    /* allocate */
    _nodeList = new BVNode[_nodeNum];
    FOR(I,0,_nodeNum) _nodeList[I]._tree = this;
    _leafIndexList = new Int32[_leafNum];
    /* first and last node index */
    Int32 power = 0, tmp = _leafNum - 1;
    while(tmp > 0) {
        tmp >>= 1;
        ++power;
    }
    Int32 maxLeafNum = 1 << power;
    _firstLeafIndex = (1 << power) - 1;
    Int32 minLeafIndex = _nodeNum >> 1; // the minimum index leaf is in middle of array
    if(minLeafIndex == _firstLeafIndex) ++power;
    _lastLeafIndex = (1 << power) - 2;
    /* place the leaf element into _leafIndexList */
    Int32 curIndex = _leafNum * 2 - maxLeafNum;
    if(_leafNum == maxLeafNum) curIndex = 0;
    FOR(I,0,_leafNum) {
        _leafIndexList[I] = leafIndex[curIndex++];
        if(curIndex >= _leafNum)
            curIndex = 0;
    }
}

/* Arrange element by iterative division on longest axis,
 * result are saved in the order index list leafIndex,
 * in the form of leaf nodes of the corresponding complete binary tree.
 */
void BVTree::leafArrangement( const vector<Vec3f>& elementPostions, vector<Int32>& leafIndex ) const
{
#define ELEMENT_POSITION(id)    elementPostions[ leafIndex[id] ]

    typedef pair<Int32,Int32> Range;
    queue<Range> callHeap;  // here we're using the nonrecursive mothed
    Range r(0,SZ(elementPostions)), r2;
    callHeap.push(r);
    Int32 rangeLen = 0;
    Int8  maxAxis = 0;
    Vec3f boundMax, boundMin;
    const static Int8 axisSelect[8] = { 2, 2, -1, 0, 1, -1, 1, 0 };
    Int32 tmp, division, pivotId, pivotVal;
    Float divideVal;
    Int32 lid, rid;

    while(!callHeap.empty()) {
        r = callHeap.front();
        callHeap.pop();
        rangeLen = r.second - r.first;
        if(rangeLen < 3) {
            if(rangeLen == 2) {
                if(ELEMENT_POSITION(r.first) > ELEMENT_POSITION(r.first+1))
                    swap(leafIndex[r.first],leafIndex[r.first+1]);
            }
            continue; // no need to go deep
        }

        // max bound axis selection
        boundMax = boundMin = ELEMENT_POSITION(r.first);
        FOR(pid,r.first+1,r.second) {
            const Vec3f& pos = ELEMENT_POSITION(pid);
            FOR(axis,0,3) {
                boundMin[axis] = min(boundMin[axis], pos[axis]);
                boundMax[axis] = max(boundMax[axis], pos[axis]);
            }
        }
        boundMax -= boundMin; // boundMax now saves boundRange
        tmp = 0;
        if(boundMax.x > boundMax.y) tmp |= 1;
        if(boundMax.x > boundMax.z) tmp |= 2;
        if(boundMax.y > boundMax.z) tmp |= 4;
        maxAxis = axisSelect[tmp];

        // arrangement
        // we're using the O(n) algorithm to reorder array into two part,
        // the smaller and bigger parts.
        division = completeBinaryTreeLeafDivide(rangeLen);
        division += r.first;
        r2 = r;
        while(TRUE) {
            pivotId = math::RndRange(r2.first, r2.second);
            //pivotId = r2.first; 
            pivotVal = leafIndex[pivotId];
            divideVal = ELEMENT_POSITION(pivotId)[maxAxis];
            leafIndex[pivotId] = leafIndex[r2.first];
            lid = r2.first;
            rid = r2.second - 1;
            while(lid < rid) {
                while(lid < rid && ELEMENT_POSITION(rid)[maxAxis] >= divideVal) --rid;
                leafIndex[lid] = leafIndex[rid];
                while(lid < rid && ELEMENT_POSITION(lid)[maxAxis] <= divideVal) ++lid;
                leafIndex[rid] = leafIndex[lid];
            }
            leafIndex[lid] = pivotVal;
            if(lid == division) break;
            else if(lid > division) r2.second = lid;
            else r2.first = lid + 1;
        };

        // the sub tree process
        r2.first = r.first;
        r2.second = division;
        callHeap.push(r2);
        r2.first = division;
        r2.second = r.second;
        callHeap.push(r2);
    }

#undef ELEMENT_POSITION
}

/* Divide leaf into two part (the left and right sub tree),
 * according to the arrangement of complete binary tree.
 * leafNum : the tree leaf number
 * return value: the left sub tree leaf number
 */
Int32 BVTree::completeBinaryTreeLeafDivide( Int32 leafNum ) const
{
    // the full binary tree case
    if((leafNum & leafNum - 1) == 0) return leafNum / 2;
    // the other (common) case
    int childMax = 1;
    while((childMax << 1) < leafNum) childMax <<= 1;
    int childMin = childMax >> 1;
    int res = leafNum - childMin;
    if(res > childMax) res = childMax;
    return res;
}

BV_AABB BV_AABB::Merge( const BV_AABB& first, const BV_AABB& second )
{
    BV_AABB res(first);

    if(res._max.x < second._max.x) res._max.x = second._max.x;
    if(res._max.y < second._max.y) res._max.y = second._max.y;
    if(res._max.z < second._max.z) res._max.z = second._max.z;

    if(res._min.x > second._min.x) res._min.x = second._min.x;
    if(res._min.y > second._min.y) res._min.y = second._min.y;
    if(res._min.z > second._min.z) res._min.z = second._min.z;

    return res;
}

Boolean BV_AABB::Intersect( const BV_AABB& first, const BV_AABB& second )
{
    return !( first._min.x > second._max.x || first._max.x < second._min.x ||
              first._min.y > second._max.y || first._max.y < second._min.y ||
              first._min.z > second._max.z || first._max.z < second._min.z );
}

NormalCone NormalCone::Merge( const NormalCone& first, const NormalCone& second )
{
    /* assertions */
    BOOST_ASSERT( abs(first._direction.Length() - 1) < math::Float_Epsilon );
    BOOST_ASSERT( abs(second._direction.Length() - 1) < math::Float_Epsilon );
    /* directly return invalid normal cone when first or second is invalid */
    if(!Validate(first) || !Validate(second))
        return NormalCone(Vec3f(),Float(math::PI));
    /* get the angle between two normal vector */
    Float normalAngle = acos(first._direction * second._direction);
    /* check whether one NormalCone is enclosed by another */
    if( normalAngle + first._angle < second._angle ) return second;
    if( normalAngle + second._angle < first._angle ) return first;
    /* new NormalCone get by merge */
    Float angle = (normalAngle + first._angle + second._angle) / 2;
    Float beta = angle - first._angle;
    Float cosBeta = cos(beta);
    /* Here we are using Binary Search for a best new direction */
    const static Float Threshold = 0.995f;
    Vec3f firstBound = first._direction;
    Vec3f secondBound = second._direction;
    Vec3f direction;
    do {
        direction = firstBound + secondBound;
        direction.Normalize();
        if(direction * first._direction > cosBeta)
            firstBound = direction;
        else
            secondBound = direction;
    } while(firstBound * secondBound < Threshold);
    /* result ready */
    return NormalCone(direction, angle);
}

MeshFace BVNode::GetLeafMeshFace() const
{
    Int32 leafIndex = getLeafIndex();
    Int32 faceIndex = _tree->_leafIndexList[leafIndex];
    return _tree->_mesh->GetFace(faceIndex);
}

Int32 BVNode::GetDepth() const
{
    Int32 index = getNodeIndex() + 1;
    Int32 depth = -1;
    while(index != 0) {
        ++depth;
        index >>= 1;
    }
    return depth;
}

void BVNode::Update()
{
    if(!isLeaf()) {
        // in our case, every internal node has both leaf and right child,
        // so we merge BVs of both childs.
        BVNode* lchild = GetChildLeft();
        BVNode* rchild = GetChildRight();
        _bv = BV_AABB::Merge(lchild->_bv, rchild->_bv);
        _nc = NormalCone::Merge(lchild->_nc, rchild->_nc);
    } else {
        // get the bv of a triangle
        MeshFace triangle = GetLeafMeshFace();
        Vec3f& p0 = triangle.GetVertex(0).GetRef();
        Vec3f max(p0);
        Vec3f min(max);
        Vec3f& p1 = triangle.GetVertex(1).GetRef();
        if(p1.x > max.x) max.x = p1.x; else if(p1.x < min.x) min.x = p1.x;
        if(p1.y > max.y) max.y = p1.y; else if(p1.y < min.y) min.y = p1.y;
        if(p1.z > max.z) max.z = p1.z; else if(p1.z < min.z) min.z = p1.z;
        Vec3f& p2 = triangle.GetVertex(2).GetRef();
        if(p2.x > max.x) max.x = p2.x; else if(p2.x < min.x) min.x = p2.x;
        if(p2.y > max.y) max.y = p2.y; else if(p2.y < min.y) min.y = p2.y;
        if(p2.z > max.z) max.z = p2.z; else if(p2.z < min.z) min.z = p2.z;
        // add a tiny epsilon
        max.x += math::Float_Epsilon; max.y += math::Float_Epsilon; max.z += math::Float_Epsilon;
        min.x -= math::Float_Epsilon; min.y -= math::Float_Epsilon; min.z -= math::Float_Epsilon;
        // update bv
        _bv.Update(max, min);

        // get the normal of a triangle
        Vec3f normal = triangle.GetNormal();
        Float angle = math::Float_Epsilon; // add a tiny epsilon
        _nc.Update(normal, angle);
    }
}

#endif
